#include <map>
#include "Whomper.h"

void
Whomper::setExclusiveLED(LED *led)
{
	setAllLEDs(false);
	led->TurnOn();
}

void
Whomper::setAllLEDs(bool shouldBeOn)
{
#if 0
	for(int i = 0;i < kLEDCount;i++)
	{
		if(shouldBeOn)
			m_LEDs[i]->TurnOn();
		else
			m_LEDs[i]->TurnOff();
	}
#endif
}

int
Whomper::getLiftTarget(void)
{
#if 0
	switch(m_highSwitch->GetPosition())
	{
		case Switch::Positive:
			return m_armPositions[kHighPlacePositionNumber];
			break;
		case Switch::Negative:
			return m_armPositions[kHighRemovePositionNumber];
			break;
	}
	
	switch(m_midSwitch->GetPosition())
	{
		case Switch::Positive:
			return m_armPositions[kMidPlacePositionNumber];
			break;
		case Switch::Negative:
			return m_armPositions[kMidRemovePositionNumber];
			break;
	}
	
	switch(m_lowSwitch->GetPosition())
	{
		case Switch::Positive:
			return m_armPositions[kLowPlacePositionNumber];
			break;
		case Switch::Negative:
			return m_armPositions[kLowRemovePositionNumber];
			break;
	}
	
	switch(m_travelSwitch->GetPosition())
	{
		case Switch::Positive:
			return m_armPositions[kTravelPositionNumber];
			break;
		case Switch::Negative:
			return m_armPositions[kGroundPositionNumber];
			break;
	}
#endif
	return kArmDoNotMove;
}

void
Whomper::doTeachMode(void)
{
	
}

Whomper::Whomper(void)
{
#if 0
	m_LEDs[kNeutralLEDNumber]    = new LED(kNeutralLEDChannel);
	m_LEDs[kHighPlaceLEDNumber]  = new LED(kHighPlaceLEDChannel);
	m_LEDs[kMidPlaceLEDNumber]	 = new LED(kMidPlaceLEDChannel);
	m_LEDs[kLowPlaceLEDNumber]	 = new LED(kLowPlaceLEDChannel);
	m_LEDs[kTravelLEDNumber]	 = new LED(kTravelLEDChannel);
	m_LEDs[kReadyLEDNumber]	     = new LED(kReadyLEDChannel);
	m_LEDs[kHighRemoveLEDNumber] = new LED(kHighRemoveLEDChannel);
	m_LEDs[kMidRemoveLEDNumber]  = new LED(kMidRemoveLEDChannel);
	m_LEDs[kLowRemoveLEDNumber]  = new LED(kLowRemoveLEDChannel);
	m_LEDs[kGroundLEDNumber]	 = new LED(kGroundLEDChannel);
#endif
	
	//TODO: Replace zeroes with EEPROM reading.
	m_armPositions[kHighPlacePositionNumber]  = 0;
	m_armPositions[kMidPlacePositionNumber]	  = 0;
	m_armPositions[kLowPlacePositionNumber]	  = 0;
	m_armPositions[kTravelPositionNumber]	  = 0;
	m_armPositions[kHighRemovePositionNumber] = 0;
	m_armPositions[kMidRemovePositionNumber]  = 0;
	m_armPositions[kLowRemovePositionNumber]  = 0;
	m_armPositions[kGroundPositionNumber]	  = 0;
	
	m_armTarget = m_armPositions[kGroundPositionNumber];
	
	//Inputs
	m_leftJoystick  = new Joystick(kLeftJoystickChannel);
	m_rightJoystick = new Joystick(kRightJoystickChannel);
	m_armJoystick   = new Joystick(kArmJoystickChannel);
	

	m_switches[kGrabberSwitchNumber] = new TwoPositionSwitch(kGrabberSwitchChannel, kGrabberSwitchNumber);
	m_switches[kTeachKeyNumber]      = new TwoPositionSwitch(kTeachKeyChannel, kTeachKeyNumber);
#if 0
	m_switches[kHighSwitchNumber]    = new ThreePositionSwitch(kHighSwitchChannel, kHighSwitchNumber);
	m_switches[kMedSwitchNumber]     = new ThreePositionSwitch(kMedSwitchChannel, kMedSwitchNumber);
	m_switches[kLowSwitchNumber]     = new ThreePositionSwitch(kLowSwitchChannel, kLowSwitchNumber);
	m_switches[kTravelSwitchNumber]  = new ThreePositionSwitch(kTravelSwitchChannel, kTravelSwitchNumber);
	
	m_switches[kHighSwitchNumber]->AddListener(this);
	m_switches[kMedSwitchNumber]->AddListener(this);
	m_switches[kLowSwitchNumber]->AddListener(this);
	m_switches[kTravelSwitchNumber]->AddListener(this);
#endif
	
	//Outputs
	m_compressor = new Compressor(kPressureSwitchChannel, kCompressorChannel);
	m_driveTrain = new RobotDrive(kDriveLeftMotorChannel, kDriveRightMotorChannel);
	m_lift		 = new Lift(kLiftLeftMotorChannel, kLiftRightMotorChannel, kLiftHeightChannel);
	m_grabber	 = new Grabber(kGrabberChannel, true);
	
	//Noputs
	m_disabled_periodic_loop_count = 0;
	m_auto_periodic_loop_count	   = 0;
	m_tele_periodic_loop_count	   = 0;
}

void
Whomper::DisabledInit(void)
{
	m_disabled_periodic_loop_count = 0;
}

void
Whomper::AutonomousInit(void)
{
	m_auto_periodic_loop_count	   = 0;
}

void
Whomper::TeleopInit(void)
{
	m_tele_periodic_loop_count	   = 0;
}
	
void
Whomper::DisabledPeriodic(void)
{
	
}

void
Whomper::AutonomousPeriodic(void)
{
#if 0
	static int curLED = 0;
	setExclusiveLED(m_LEDs[curLED]);
	if(++curLED == kLEDCount)
		curLED = 0;
#endif
	
	if(m_compressor->GetPressureSwitchValue())
		m_compressor->Stop();
	else
		m_compressor->Start();
}

void
Whomper::TeleopPeriodic(void)
{
#if 0
	if(m_switches[kTeachKeyNumber]->GetPosition() == Switch::Positive)
	{
		doTeachMode();
		return;
	}
#endif
	
	float left = m_leftJoystick->GetAxis(Joystick::kYAxis);
	float right = m_rightJoystick->GetAxis(Joystick::kYAxis);
	float arm = m_armJoystick->GetAxis(Joystick::kYAxis);
#if 0
	bool  trigger = m_armJoystick->GetTrigger();
#endif
	m_driveTrain->TankDrive(left, right);
#if 0
	if(trigger) //Driving arm with joystick
	{
		m_lift->ManualDrive(arm);

	}
	else //Driving arm with switches
	{
		m_lift->DriveTo(m_armTarget);
	}
#endif
	
	if(m_switches[kGrabberSwitchNumber]->GetPosition() == Switch::Positive)
		m_grabber->Grab();
	else
		m_grabber->Release();
}

void
Whomper::DisabledContinuous(void)
{
	
}

void
Whomper::AutonomousContinuous(void)
{
	
}

void
Whomper::TeleopContinuous(void)
{
	//TODO: switch watcher
}

void
Whomper::SwitchChanged(int id)
{
#if 0
	std::map<Switch::Position, int> m_positionToIntMap;
	m_positionToIntMap[Switch::Positive] = 1;
	m_positionToIntMap[Switch::Neutral]  = -1;
	m_positionToIntMap[Switch::Negative] = 0;

	//2row + col = arm heigh between 0 and 7
	m_armTarget = (m_armPositions[(2 * m_positionToIntMap[m_switches[id]->GetPosition()]) + id]);
#endif
}

